#include <memory>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

#include <pluginlib/class_loader.hpp>

#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_interface/planning_interface.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit_msgs/msg/display_trajectory.hpp>
#include <moveit_msgs/msg/planning_scene.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <sensor_msgs/msg/joint_state.hpp>
#include <yyrobot_msg/msg/arm_goal_state.hpp>

class BlRobotCtrl : public rclcpp::Node
{
public:
  const std::string PLANNING_GROUP = "the_arm";
  BlRobotCtrl();;
  ~BlRobotCtrl();

private:
  
  rclcpp::Subscription<yyrobot_msg::msg::ArmGoalState>::SharedPtr subscription_ = NULL;
  //
  std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::JointState>> jointStatePublisher_ = NULL;
  // 
  std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::JointState>> jointStatePublisher_2 = NULL;
  moveit::core::RobotModelPtr robot_model_ptr = NULL;

  moveit::core::RobotStatePtr robot_statePtr = NULL;
  const moveit::core::JointModelGroup* joint_model_group = NULL;
  planning_scene::PlanningScenePtr planning_scene = NULL;

  std::unique_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager>> planner_plugin_loader;
  planning_interface::PlannerManagerPtr planner_instance=NULL;
  std::string planner_plugin_name = "ompl_interface/OMPLPlanner";

  std::vector<std::string> joint_names;

  std::shared_ptr<moveit_msgs::msg::DisplayTrajectory> display_trajectoryPtr; // = NULL;
  rclcpp::Publisher<moveit_msgs::msg::DisplayTrajectory>::SharedPtr display_publisherPtr; // = NULL;
  moveit_visual_tools::MoveItVisualToolsPtr visual_toolsPtr; // = NULL;
  moveit::planning_interface::MoveGroupInterface* move_groupPtr = NULL;
  
  std::shared_ptr<robot_model_loader::RobotModelLoader> robot_model_loaderPtr = NULL;

  rclcpp::TimerBase::SharedPtr timer_ptr_;
  void topic_callback(const yyrobot_msg::msg::ArmGoalState& msg);
  void doControl();
  void init();
  void plan(geometry_msgs::msg::PoseStamped* posePtr);
};

